

#ifndef __INKA_MATH_MATRIX_H__
#define __INKA_MATH_MATRIX_H__

#include <math/vector.h>
#include <stdio.h>

namespace inka
{
namespace math
{

struct Matrix4f
{
  union
  {
    struct
    {
      float _00;
      float _01;
      float _02;
      float _03;
      float _10;
      float _11;
      float _12;
      float _13;
      float _20;
      float _21;
      float _22;
      float _23;
      float _30;
      float _31;
      float _32;
      float _33;
    };
    float m[16];
  };

  inline Matrix4f ()
  {
    SetIdentity ();
  }

  inline Matrix4f (const Vector3f& x, const Vector3f& y, const Vector3f& z)
  {
    Set (x, y, z);
  }

  inline Matrix4f (float m00, float m01, float m02, float m03,
                  float m10, float m11, float m12, float m13,
                  float m20, float m21, float m22, float m23,
                  float m30, float m31, float m32, float m33)
    : _00(m00)
    , _01(m01)
    , _02(m02)
    , _03(m03)
    , _10(m10)
    , _11(m11)
    , _12(m12)
    , _13(m13)
    , _20(m20)
    , _21(m21)
    , _22(m22)
    , _23(m23)
    , _30(m30)
    , _31(m31)
    , _32(m32)
    , _33(m33)
  {

  }

  inline void Set (const Vector3f& x, const Vector3f& y, const Vector3f& z)
  {
    _00 = x.x;
    _01 = x.y;
    _02 = x.z;
    _03 = 0.0f;

    _10 = y.x;
    _11 = y.y;
    _12 = y.z;
    _13 = 0.0f;

    _20 = z.x;
    _21 = z.y;
    _22 = z.z;
    _23 = 0.0f;

    _30 = 0.0f;
    _31 = 0.0f;
    _32 = 0.0f;
    _33 = 1.0f;

  }

  inline void Set (float m00, float m01, float m02, float m03,
                   float m10, float m11, float m12, float m13,
                   float m20, float m21, float m22, float m23,
                   float m30, float m31, float m32, float m33)
  {
    _00 = m00;
    _01 = m01;
    _02 = m02;
    _03 = m03;
    _10 = m10;
    _11 = m11;
    _12 = m12;
    _13 = m13;
    _20 = m20;
    _21 = m21;
    _22 = m22;
    _23 = m23;
    _30 = m30;
    _31 = m31;
    _32 = m32;
    _33 = m33;
  }


  inline void SetIdentity ()
  {
    _01 = _02 = _03 = _10 = _12 = _13 = _20 = _21 = _23 = _30 = _31 = _32 = 0.0f;
    _00 = _11 = _22 = _33 = 1.0f;
  }


  inline Matrix4f& Transpose ()
  {
    float t;
#define SWAP(x,y) t=x; x=y; y=t
    SWAP(_01, _10);
    SWAP(_02, _20);
    SWAP(_03, _30);

    SWAP(_12, _21);
    SWAP(_13, _31);

    SWAP(_23, _32);
#undef SWAP
    return *this;
  }

  inline Matrix4f Transposed () const
  {
    return Matrix4f (_00, _10, _20, _30,
                    _01, _11, _21, _31,
                    _02, _12, _22, _32,
                    _03, _13, _23, _33);
  }

  inline void FastInvert ()
  {
    float x = _30;
    float y = _31;
    float z = _32;

    Transpose ();

    float m30 = -(_00 * x + _10 * y + _20 * z);
    float m31 = -(_01 * x + _11 * y + _21 * z);
    float m32 = -(_02 * x + _12 * y + _22 * z);


    _30 = m30;
    _31 = m31;
    _32 = m32;
    _03 = 0.0f;
    _13 = 0.0f;
    _23 = 0.0f;
  }

  inline Matrix4f FastInverted () const
  {
    float m00 = _00;
    float m01 = _10;
    float m02 = _20;
    float m10 = _01;
    float m11 = _11;
    float m12 = _21;
    float m20 = _02;
    float m21 = _12;
    float m22 = _22;


    float x = _30;
    float y = _31;
    float z = _32;

    float m30 = -(m00 * x + m10 * y + m20 * z);
    float m31 = -(m01 * x + m11 * y + m21 * z);
    float m32 = -(m02 * x + m12 * y + m22 * z);

    return Matrix4f (m00, m01, m02, 0.0f,
                    m10, m11, m12, 0.0f,
                    m20, m21, m22, 0.0f,
                    m30, m31, m32, 1.0f);
  }

  /**
   * \brief Returns the inverted matrix.
   *
   * \return The inverted matrix.
   */
  inline Matrix4f Inverted () const
  {
    float v0 = _20 * _31 - _21 * _30;
    float v1 = _20 * _32 - _22 * _30;
    float v2 = _20 * _33 - _23 * _30;
    float v3 = _21 * _32 - _22 * _31;
    float v4 = _21 * _33 - _23 * _31;
    float v5 = _22 * _33 - _23 * _32;

    float t00 = +(v5 * _11 - v4 * _12 + v3 * _13);
    float t10 = -(v5 * _10 - v2 * _12 + v1 * _13);
    float t20 = +(v4 * _10 - v2 * _11 + v0 * _13);
    float t30 = -(v3 * _10 - v1 * _11 + v0 * _12);

    float invDet = 1.0f / (t00 * _00 + t10 * _01 + t20 * _02 + t30 * _03);

    float d00 = t00 * invDet;
    float d10 = t10 * invDet;
    float d20 = t20 * invDet;
    float d30 = t30 * invDet;

    float d01 = -(v5 * _01 - v4 * _02 + v3 * _03) * invDet;
    float d11 = +(v5 * _00 - v2 * _02 + v1 * _03) * invDet;
    float d21 = -(v4 * _00 - v2 * _01 + v0 * _03) * invDet;
    float d31 = +(v3 * _00 - v1 * _01 + v0 * _02) * invDet;

    v0 = _10 * _31 - _11 * _30;
    v1 = _10 * _32 - _12 * _30;
    v2 = _10 * _33 - _13 * _30;
    v3 = _11 * _32 - _12 * _31;
    v4 = _11 * _33 - _13 * _31;
    v5 = _12 * _33 - _13 * _32;

    float d02 = +(v5 * _01 - v4 * _02 + v3 * _03) * invDet;
    float d12 = -(v5 * _00 - v2 * _02 + v1 * _03) * invDet;
    float d22 = +(v4 * _00 - v2 * _01 + v0 * _03) * invDet;
    float d32 = -(v3 * _00 - v1 * _01 + v0 * _02) * invDet;

    v0 = _21 * _10 - _20 * _11;
    v1 = _22 * _10 - _20 * _12;
    v2 = _23 * _10 - _20 * _13;
    v3 = _22 * _11 - _21 * _12;
    v4 = _23 * _11 - _21 * _13;
    v5 = _23 * _12 - _22 * _13;

    float d03 = -(v5 * _01 - v4 * _02 + v3 * _03) * invDet;
    float d13 = +(v5 * _00 - v2 * _02 + v1 * _03) * invDet;
    float d23 = -(v4 * _00 - v2 * _01 + v0 * _03) * invDet;
    float d33 = +(v3 * _00 - v1 * _01 + v0 * _02) * invDet;

    return Matrix4f (d00, d01, d02, d03, d10, d11, d12, d13, d20, d21, d22, d23, d30, d31, d32, d33);

  }

  inline void SetXAxis (const Vector3f& x)
  {
    _00 = x.x;
    _01 = x.y;
    _02 = x.z;
  }

  inline Vector3f GetXAxis () const
  {
    return Vector3f (_00, _01, _02);
  }

  inline void SetYAxis (const Vector3f& y)
  {
    _10 = y.x;
    _11 = y.y;
    _12 = y.z;
  }

  inline Vector3f GetYAxis () const
  {
    return Vector3f (_10, _11, _12);
  }

  inline void SetZAxis (const Vector3f& z)
  {
    _20 = z.x;
    _21 = z.y;
    _22 = z.z;
  }

  inline Vector3f GetZAxis () const
  {
    return Vector3f (_20, _21, _22);
  }

  inline void SetTranslation(float x, float y, float z)
  {
    _30 = x;
    _31 = y;
    _32 = z;
  }

  inline void SetTranslation (const Vector3f& t)
  {
    _30 = t.x;
    _31 = t.y;
    _32 = t.z;
  }

  inline Vector3f GetTranslation () const
  {
    return Vector3f (_30, _31, _32);
  }

  inline void SetRotationX (float angle)
  {
    float c = (float)cos (angle);
    float s = (float)sin (angle);
    _11 = c;
    _12 = s;
    _21 = -s;
    _22 = c;
  }

  inline void SetRotationY (float angle)
  {
    float c = (float)cos (angle);
    float s = (float)sin (angle);
    _00 = c;
    _02 = -s;
    _20 = s;
    _22 = c;
  }

  inline void SetRotationZ (float angle)
  {
    float c = (float)cos (angle);
    float s = (float)sin (angle);
    _00 = c;
    _01 = s;
    _10 = -s;
    _11 = c;
  }

  inline void SetRotation (const Vector3f& axis, float angle)
  {
    float a = angle * (float) M_PI / 180.0f;
    float c = cosf (a);
    float s = sinf (a);

    // TODO: This is wast of computation time. Use a pre-normalized axis
    Vector3f ax = axis.Normalized ();

    float x = ax.x;
    float y = ax.y;
    float z = ax.z;

    float ic = 1.0f - c;
    _00 = x * x * ic + c;
    _10 = x * y * ic - z * s;
    _20 = x * z * ic + y * s;
    _01 = y * x * ic + z * s;
    _11 = y * y * ic + c;
    _21 = y * z * ic - x * s;
    _02 = x * z * ic - y * s;
    _12 = y * z * ic + x * s;
    _22 = z * z * ic + c;
  }

  inline Matrix4f operator* (const Matrix4f& m) const
  {
    return Matrix4f (_00 * m._00 + _10 * m._01 + _20 * m._02 + _30 * m._03,
                    _01 * m._00 + _11 * m._01 + _21 * m._02 + _31 * m._03,
                    _02 * m._00 + _12 * m._01 + _22 * m._02 + _32 * m._03,
                    _03 * m._00 + _13 * m._01 + _23 * m._02 + _33 * m._03,

                    _00 * m._10 + _10 * m._11 + _20 * m._12 + _30 * m._13,
                    _01 * m._10 + _11 * m._11 + _21 * m._12 + _31 * m._13,
                    _02 * m._10 + _12 * m._11 + _22 * m._12 + _32 * m._13,
                    _03 * m._10 + _13 * m._11 + _23 * m._12 + _33 * m._13,

                    _00 * m._20 + _10 * m._21 + _20 * m._22 + _30 * m._23,
                    _01 * m._20 + _11 * m._21 + _21 * m._22 + _31 * m._23,
                    _02 * m._20 + _12 * m._21 + _22 * m._22 + _32 * m._23,
                    _03 * m._20 + _13 * m._21 + _23 * m._22 + _33 * m._23,

                    _00 * m._30 + _10 * m._31 + _20 * m._32 + _30 * m._33,
                    _01 * m._30 + _11 * m._31 + _21 * m._32 + _31 * m._33,
                    _02 * m._30 + _12 * m._31 + _22 * m._32 + _32 * m._33,
                    _03 * m._30 + _13 * m._31 + _23 * m._32 + _33 * m._33);

  }

  inline Vector3f operator*(const Vector3f& v) const
  {
    return Vector3f (_00 * v.x + _10 * v.y + _20 * v.z + _30,
                    _01 * v.x + _11 * v.y + _21 * v.z + _31,
                    _02 * v.x + _12 * v.y + _22 * v.z + _32);
  }

  inline Vector4f operator*(const Vector4f& v) const
  {
    return Vector4f (_00 * v.x + _10 * v.y + _20 * v.z + _30 * v.w,
                    _01 * v.x + _11 * v.y + _21 * v.z + _31 * v.w,
                    _02 * v.x + _12 * v.y + _22 * v.z + _32 * v.w,
                    _03 * v.x + _13 * v.y + _23 * v.z + _33 * v.w);
  }

  inline Vector3f Rotate (const Vector3f& v) const
  {
    return Vector3f (_00 * v.x + _10 * v.y + _20 * v.z,
                    _01 * v.x + _11 * v.y + _21 * v.z,
                    _02 * v.x + _12 * v.y + _22 * v.z);
  }

  inline Vector3f Project (const Vector3f& v) const
	{
		float x = _00 * v.x + _10 * v.y + _20 * v.z + _30;
		float y = _01 * v.x + _11 * v.y + _21 * v.z + _31;
		float z = _02 * v.x + _12 * v.y + _22 * v.z + _32;
		float w = _03 * v.x + _13 * v.y + _23 * v.z + _33;
    return Vector3f (x / w, y / w, z / w);
	}

  inline void LookAt (const Vector3f& at, const Vector3f& up = Vector3f (0.0f, 1.0f, 0.0f))
  {
    Vector3f z (at - Vector3f (_30, _31, _32));
    z.Normalize();

    Vector3f y (up - z * (up.Dot(z)));
    y.Normalize();

    Vector3f x (y % z);

    SetXAxis(x);
    SetYAxis(y);
    SetZAxis(z);
  }

  inline void SetLookAt (const Vector3f& eye, const Vector3f& spot, const Vector3f& up)
  {
    Vector3f z (eye - spot);
    z.Normalize();

    Vector3f y (up - z * (up.Dot(z)));
    y.Normalize();

    Vector3f x (y % z);

    // Set the matrix directly transposed
    SetXAxis(Vector3f (x.x, y.x, z.x));
    SetYAxis(Vector3f (x.y, y.y, z.y));
    SetZAxis(Vector3f (x.z, y.z, z.z));

    // Set the negated translation
    Vector3f e = Rotate (eye);
    SetTranslation(Vector3f (-e.x, -e.y, -e.z));
  }

  inline void SetPerspective(float left, float right, float bottom, float top, float fnear, float ffar)
  {
    float z2 = 2.0f * fnear;
    float dx = right - left;
    float dy = top - bottom;
    float dz = ffar - fnear;
    float sx = right + left;
    float sy = top + bottom;
    float sz = fnear + ffar;

    _00 = z2 / dx; _10 = 0.0f;    _20 = sx / dx;  _30 = 0.0f;
    _01 = 0.0f;    _11 = z2 / dy; _21 = sy / dy;  _31 = 0.0f;
    _02 = 0.0f;    _12 = 0.0f;    _22 = -sz / dz; _32 = -2.0f*fnear*ffar / dz;
    _03 = 0.0f;    _13 = 0.0f;    _23 = -1.0f;    _33 = 0.0f;
  }

  inline void SetPerspectiveInv (float left, float right, float bottom, float top, float fnear, float ffar)
  {
    float z2 = 2.0f * fnear;
    float dx = right - left;
    float dy = top - bottom;
    float dz = ffar - fnear;
    float sx = right + left;
    float sy = top + bottom;
    float sz = fnear + ffar;
    float nf2 = 2.0f * fnear * ffar;

    _00 = dx / z2; _10 = 0.0f;    _20 = 0.0f;       _30 = sx / z2;
    _01 = 0.0f;    _11 = dy / z2; _21 = 0.0f;       _31 = sy / z2;
    _02 = 0.0f;    _12 = 0.0f;    _22 = 0.0f;       _32 = -1.0f;
    _03 = 0.0f;    _13 = 0.0f;    _23 = -dz / nf2;  _33 = sz / nf2;
  }

  inline void SetOrtho (float left, float right, float bottom, float top, float fnear, float ffar)
  {
    float dx = right - left;
    float dy = top - bottom;
    float dz = ffar - fnear;
    float sx = right + left;
    float sy = top + bottom;
    float sz = fnear + ffar;

    _00 = 2.0f / dx; _10 = 0.0f;      _20 = 0.0f;       _30 = -sx / dx;
    _01 = 0.0f;      _11 = 2.0f / dy; _21 = 0.0f;       _31 = -sy / dy;
    _02 = 0.0f;      _12 = 0.0f;      _22 = -2.0f / dz; _32 = -sz / dz;
    _03 = 0.0f;      _13 = 0.0f;      _23 = 0.0f;       _33 = 1.0f;
  }

  inline void SetOrthoInv (float left, float right, float bottom, float top, float fnear, float ffar)
  {
    float dx = right - left;
    float dy = top - bottom;
    float dz = ffar - fnear;
    float sx = right + left;
    float sy = top + bottom;
    float sz = fnear + ffar;

    _00 = dx / 2.0f; _10 = 0.0f;      _20 = 0.0f;       _30 = sx / 2.0f;
    _01 = 0.0f;      _11 = dy / 2.0f; _21 = 0.0f;       _31 = sy / 2.0f;
    _02 = 0.0f;      _12 = 0.0f;      _22 = dz / -2.0f; _32 = sz / 2.0f;
    _03 = 0.0f;      _13 = 0.0f;      _23 = 0.0f;       _33 = 1.0f;
  }

  inline void Debug (const char* message = 0) const
  {
    if (message)
      {
        printf ("Matrix: %s\n", message);
      }
    else
      {
        printf ("Matrix:\n");
      }
    printf ("  %.2f  %.2f  %.2f  %.2f\n", _00, _01, _02, _03);
    printf ("  %.2f  %.2f  %.2f  %.2f\n", _10, _11, _12, _13);
    printf ("  %.2f  %.2f  %.2f  %.2f\n", _20, _21, _22, _23);
    printf ("  %.2f  %.2f  %.2f  %.2f\n", _30, _31, _32, _33);
    fflush(stdout);
  }
};

}
}

#endif /* ! __INKA_MATH_MATRIX_H__ */
